/*
 * Math.h
 *
 *  Created on: Jan 21, 2014
 *      Author: ppks
 */

#ifndef MATH_H_
#define MATH_H_

#include "Arduino.h"
#include "types.h"

/********************************************************************************/
/* Macro Definition                                                             */
/********************************************************************************/
/********************************************************************************/
/* Macro Definition                                                             */
/********************************************************************************/
/* signed16 x signed16 */
/* 22 cycles */
/* http://mekonik.wordpress.com/2009/03/18/arduino-avr-gcc-multiplication/ */
#define MultiS16X16to32(longRes, intIn1, intIn2) \
asm volatile ( \
    "clr r26 \n\t"          \
    "mul %A1, %A2 \n\t"     \
    "movw %A0, r0 \n\t"     \
    "muls %B1, %B2 \n\t"    \
    "movw %C0, r0 \n\t"     \
    "mulsu %B2, %A1 \n\t"   \
    "sbc %D0, r26 \n\t"     \
    "add %B0, r0 \n\t"      \
    "adc %C0, r1 \n\t"      \
"adc %D0, r26 \n\t"         \
    "mulsu %B1, %A2 \n\t"   \
    "sbc %D0, r26 \n\t"     \
    "add %B0, r0 \n\t"      \
    "adc %C0, r1 \n\t"      \
"adc %D0, r26 \n\t"         \
    "clr r1 \n\t"           \
    :                       \
    "=&r" (longRes)         \
    :                       \
    "a" (intIn1),           \
    "a" (intIn2)            \
    :                       \
    "r26"                   \
)

/********************************************************************************/
/* Exported Functions Prototypes                                                */
/********************************************************************************/
extern int32_t  __attribute__ ((noinline)) mul(int16_t a, int16_t b);
extern int16_t _atan2(int32_t y, int32_t x);
extern float InvSqrt(float x);

#endif /* MATH_H_ */
